#include <UTFT.h>
#include <math.h>
extern uint8_t BigFont[];

UTFT myGLCD(ITDB32S,19,18,17,16);  
double pi = 3.14;
double R = 119.0;
void setup()
{
  randomSeed(analogRead(0));
  
  myGLCD.InitLCD();
  myGLCD.setFont(BigFont);
  
}
void loop()
{

  myGLCD.clrScr();
  int Xsize = myGLCD.getDisplayXSize();
  int Ysize = myGLCD.getDisplayYSize();
  int Xmid = Xsize/2;
  int Ymid = Ysize/2;

  myGLCD.setColor(255, 0, 0);
  myGLCD.drawCircle( Xmid,  Ymid, 119);
  myGLCD.setColor(205, 133, 63);
  
  int x0 = Xmid;
  int y0 = Ymid;
  
  /*double angle1 = 0;  
  double x1 = x0 + R * cos( angle1 );
  double y1 = y0 + R * sin( angle1 );
  myGLCD.print("0", x1+10, y1,0 );
  
  double angle2 = (90/180.0)*pi;
  double x2 = x0 + R * cos( angle2 );
  double y2 = y0 + R * sin( angle2 );
     
  myGLCD.setColor(255, 0, 0);
  myGLCD.setColor(132, 112, 255);
  myGLCD.print("90", x2+40, y2-20, 0);
  */

  double pitchA = (10/180.0)*pi; //10
  double rollA = ((90-15)/180.0)*pi; //60
  double pitchL = 0.0;
  double arrowR = 40.0;
  int i = 0;
  
  for (int j = 0; j < 60; j+=10)
     {
     
     rollA = ((90+j)/180.0)*pi;
     double arrowA1 = ((315+j)/180.0)*pi; //330
     double arrowA2 = ((225+j)/180.0)*pi; //270 
     pitchL += 7;
     myGLCD.setColor(255,20,147);
     pitchArrow( pitchL,rollA,x0,  y0);
   
     double xR1 = x0 + (R-80) * cos(rollA);
     double yR1 = y0 + (R-80) * sin(rollA);
     
     double xR2 = x0 + R * cos(rollA);
     double yR2 = y0 + R * sin(rollA);
       
     myGLCD.drawLine(xR1,yR1,xR2,yR2); 
     
     //drawing numbers
     drawingNumbers(x0,  y0, rollA); 
     
     //drawing pitch line
     
     pitchArrow( pitchL,rollA,x0,  y0);
     
     //drawing arrow
     
     double xRa1 = xR2 + arrowR * cos(arrowA1);
     double yRa1 = yR2 + arrowR * sin(arrowA1);
     
     myGLCD.drawLine(xR2,yR2,xRa1,yRa1);     
     
     double xRa2 = xR2 + arrowR * cos(arrowA2);
     double yRa2 = yR2 + arrowR * sin(arrowA2);
     
     myGLCD.drawLine(xR2,yR2,xRa2,yRa2);
     
     
     
     delay(1000);  
  
     // Clearing screan   
     myGLCD.setColor(0,0,0);
     myGLCD.drawLine(xR1,yR1,xR2,yR2); 
     myGLCD.drawLine(xR2,yR2,xRa1,yRa1); 
     myGLCD.drawLine(xR2,yR2,xRa2,yRa2); 
     pitchArrow( pitchL,rollA,x0,  y0);
     drawingNumbers(x0,  y0, rollA);
     
     }  
  
  delay (8000);
}
  void pitchArrow(double pitch, double roll, double x0, double y0)
  {
  double pitchAone = (pi*2.0) - ( (asin((sqrt(pow(R, 2.0) - pow(pitch, 2.0)))/R)) + (pi/2.0) - (roll - (pi/2.0)) ); 
   
  double xPaOne = x0 + R * cos(pitchAone);
  double yPaOne = y0 + R * sin(pitchAone);

  double pitchAtwo = (pi*2.0) + ( (asin((sqrt(pow(R, 2.0) - pow(pitch, 2.0)))/R))  - ((pi/2.0) -(roll - (pi/2.0))) );

  double xPaTwo = x0 + R * cos(pitchAtwo);
  double yPaTwo = y0 + R * sin(pitchAtwo);
 
 
  myGLCD.drawLine(xPaOne,yPaOne,xPaTwo,yPaTwo);
  }
  
  void drawingNumbers(int xCenter, int yCenter,double rollAngle)
  {
    
     int numb= 15;
     for (int k = 0; k <= 65; k+=15)
     {
          
     double numbA1 = rollAngle + pi; 
     if  ( k <=35 )
     {
     numb -=5;
     }
     else
     {
     numb += 5;
     }
     double xRn1 = xCenter + k * cos(numbA1);
     double yRn1 = (yCenter-15) + k * sin(numbA1);
     myGLCD.print(String(numb), xRn1, yRn1, ((numbA1+(pi/2))/pi)*180.0 );
     }
  } 
